/*
 * @Author: LVGRAPE
 * @Date: 2023-09-13 10:28:15
 * @LastEditTime: 2024-07-08 16:59:15
 * @LastEditors: EltonLi lijiaming@uavi-tech.com
 * @Description: inc
 * @FilePath: \zino-fc-v4\ZINO\hardware\pmw3901\sensor_pixart_pmw3901mb.c
 * 要啥没啥，爱咋咋的
 */
#include "driver_pmw3901mb.h"
#include <rtdevice.h>
#include "zino.h"
#include <sensor.h>
#include "driver_pmw3901mb.h"
#include "fc_error.h"

#define DBG_TAG "dev_3901"
#define DBG_LVL DBG_LOG
#include <rtdbg.h>


#define PMW3901_BUS_NAME "spi_pmw"
#define PMW3901_NRESET_PIN 42 //PC10
#define PMW3901_P_MOTION_PIN 15 //PA15


static struct rt_spi_device* gs_bus;
// static pmw3901mb_handle_t gs_handle;        /**< pmw3901mb handle */

/**
 * @brief  interface spi bus init
 * @return status code
 *         - 0 success
 *         - 1 spi init failed
 * @note   none
 */
uint8_t pmw3901mb_interface_spi_init(void)
{
    gs_bus = (struct rt_spi_device*)rt_device_find(PMW3901_BUS_NAME);
    if (gs_bus == RT_NULL)
    {
        rt_kprintf("spi bus %s not found!\n", PMW3901_BUS_NAME);
        return 1;
    }
    gs_bus->config.data_width = 8;
    gs_bus->config.mode = RT_SPI_MASTER | RT_SPI_MSB | RT_SPI_MODE_0;
    gs_bus->config.max_hz = 20 * 1000 * 100;
    rt_spi_configure(gs_bus, &(gs_bus->config));//NOTE 调用config后才会开始设置
    return 0;
}
/**
 * @brief  interface spi bus deinit
 * @return status code
 *         - 0 success
 *         - 1 spi deinit failed
 * @note   none
 */
uint8_t pmw3901mb_interface_spi_deinit(void)
{
    // gs_bus = 0;
    return 0;
}
/**
 * @brief      interface spi bus read
 * @param[in]  reg is the register address
 * @param[out] *buf points to a data buffer
 * @param[in]  len is the length of data buffer
 * @return     status code
 *             - 0 success
 *             - 1 read failed
 * @note       none
 */
uint8_t pmw3901mb_interface_spi_read(uint8_t reg, uint8_t* buf, uint16_t len)
{
    struct rt_spi_message msg1, msg2;
    msg1.send_buf = &reg;
    msg1.recv_buf = RT_NULL;
    msg1.length = 1;
    msg1.cs_take = 1;
    msg1.cs_release = 0;
    msg1.next = &msg2;

    msg2.send_buf = RT_NULL;
    msg2.recv_buf = buf;
    msg2.length = len;
    msg2.cs_take = 1;
    msg2.cs_release = 1;
    msg2.next = RT_NULL;
    rt_spi_transfer_message(gs_bus, &msg1);
    return RT_EOK;
}
/**
 * @brief     interface spi bus write
 * @param[in] reg is the register address
 * @param[in] *buf points to a data buffer
 * @param[in] len is the length of data buffer
 * @return    status code
 *            - 0 success
 *            - 1 write failed
 * @note      none
 */
uint8_t pmw3901mb_interface_spi_write(uint8_t reg, uint8_t* buf, uint16_t len)
{
    struct rt_spi_message msg1, msg2;
    msg1.send_buf = &reg;
    msg1.recv_buf = RT_NULL;
    msg1.length = 1;
    msg1.cs_take = 1;
    msg1.cs_release = 0;
    msg1.next = &msg2;

    msg2.send_buf = buf;
    msg2.recv_buf = RT_NULL;
    msg2.length = len;
    msg2.cs_take = 1;
    msg2.cs_release = 1;
    msg2.next = RT_NULL;
    rt_spi_transfer_message(gs_bus, &msg1);
    return RT_EOK;
}
/**
 * @brief  interface reset gpio init
 * @return status code
 *         - 0 success
 *         - 1 init failed
 * @note   none
 */
uint8_t pmw3901mb_interface_reset_gpio_init(void)
{
    rt_pin_mode(PMW3901_NRESET_PIN, PIN_MODE_OUTPUT);
    rt_pin_write(PMW3901_NRESET_PIN, 1);
    return RT_EOK;
}
/**
 * @brief  interface reset gpio deinit
 * @return status code
 *         - 0 success
 *         - 1 deinit failed
 * @note   none
 */
uint8_t pmw3901mb_interface_reset_gpio_deinit(void)
{
    return 0;
}
/**
 * @brief     interface reset gpio write
 * @param[in] data is the written data
 * @return    status code
 *            - 0 success
 *            - 1 write failed
 * @note      none
 */
uint8_t pmw3901mb_interface_reset_gpio_write(uint8_t data)
{
    rt_pin_write(PMW3901_NRESET_PIN, data);
    return RT_EOK;
}
/**
 * @brief     interface delay ms
 * @param[in] ms
 * @note      none
 */
void pmw3901mb_interface_delay_ms(uint32_t ms)
{
    rt_thread_delay(ms);
}
/**
 * @brief     interface print format data
 * @param[in] fmt is the format data
 * @note      none
 */
void pmw3901mb_interface_debug_print(const char* const fmt, ...)
{
    // char str[128];
    // va_list args;

    // memset((char*)str, 0, sizeof(char) * 128);
    // va_start(args, fmt);
    // rt_vsnprintf((char*)str, 128, (char const*)fmt, args);
    // va_end(args);

    // rt_kprintf("%s", str);
}
uint8_t pmw3901_init(pmw3901mb_handle_t* gs)
{
    uint8_t ret;
    DRIVER_PMW3901MB_LINK_INIT(gs, pmw3901mb_handle_t);
    DRIVER_PMW3901MB_LINK_SPI_INIT(gs, pmw3901mb_interface_spi_init);
    DRIVER_PMW3901MB_LINK_SPI_DEINIT(gs, pmw3901mb_interface_spi_deinit);
    DRIVER_PMW3901MB_LINK_SPI_READ(gs, pmw3901mb_interface_spi_read);
    DRIVER_PMW3901MB_LINK_SPI_WRITE(gs, pmw3901mb_interface_spi_write);
    DRIVER_PMW3901MB_LINK_RESET_GPIO_INIT(gs, pmw3901mb_interface_reset_gpio_init);
    DRIVER_PMW3901MB_LINK_RESET_GPIO_DEINIT(gs, pmw3901mb_interface_reset_gpio_deinit);
    DRIVER_PMW3901MB_LINK_RESET_GPIO_WRITE(gs, pmw3901mb_interface_reset_gpio_write);
    DRIVER_PMW3901MB_LINK_DELAY_MS(gs, pmw3901mb_interface_delay_ms);
    DRIVER_PMW3901MB_LINK_DEBUG_PRINT(gs, pmw3901mb_interface_debug_print);

    // rt_kprintf("pmw3901_init...");
    // gs_handle.debug_print("hellow\n");
    ret = pmw3901mb_init(gs);
    if (ret == 0)
    {
        LOG_I("pmw3901mb_init success");

        /* chip power up */
        ret = pmw3901mb_power_up(gs);
        /* set optimum performance */
        ret = pmw3901mb_set_optimum_performance(gs);

        // pmw3901mb_motion_t motion;

        // while (1)
        // {
        //     pmw3901mb_burst_read(gs, &motion);
        //     rt_kprintf("motion x:%03d, y:%03d, is:%d obs:%d avg:%d shut:%d qua:%d\n", motion.delta_x, motion.delta_y, motion.is_valid,
        //         motion.observation,
        //         motion.raw_average,
        //         motion.shutter,
        //         motion.surface_quality
        //     );
        //     rt_thread_delay(10);
        // }

    }
    // rt_kprintf("\nRet%d \n",ret);
    return ret;

}
static pmw3901mb_handle_t* _pmw3901mb_creat(void)
{
    pmw3901mb_handle_t* gs_handle = (pmw3901mb_handle_t*)rt_malloc(sizeof(pmw3901mb_handle_t));
    if (pmw3901_init(gs_handle) != RT_EOK)
    {
        rt_free(gs_handle);
        gs_handle = RT_NULL;
    }
    return gs_handle;
}
static rt_size_t pmw3901mb_fect_data(struct rt_sensor_device* sensor, void* buf, rt_size_t len)
{
    struct rt_sensor_data* data = (struct rt_sensor_data*)buf;
    pmw3901mb_handle_t* gs = sensor->parent.user_data;
    if (sensor->info.type == RT_SENSOR_CLASS_OP)
    {
        pmw3901mb_motion_t motion;
        pmw3901mb_burst_read(gs, &motion);
        // rt_kprintf("x = %d, \t y = %d, \t valid = %d\n",motion.delta_x,motion.delta_y,motion.is_valid);

        data->type = RT_SENSOR_CLASS_OP;
        data->data.of.x = motion.delta_x;
        data->data.of.y = motion.delta_y;
        data->data.of.quality = motion.surface_quality;
        data->timestamp = rt_sensor_get_ts();
        data->data.of.shutter = motion.shutter;
        data->data.of.is_valid = motion.is_valid;
        // rt_kprintf("pmw3901mb_fect_data ret:%d\n",ret);
    }
    return 1;
}
static rt_err_t pmw3901mb_control(struct rt_sensor_device* sensor, int cmd, void* args)
{
    rt_err_t result = RT_EOK;
    pmw3901mb_handle_t* gs = sensor->parent.user_data;
    switch (cmd)
    {
    case RT_SENSOR_CTRL_GET_ID:
        break;
    case RT_SENSOR_CTRL_SET_POWER:
    {
        rt_uint32_t pctrl = (rt_uint32_t)args;
        if (pctrl == RT_SENSOR_POWER_DOWN)
        {
            LOG_D("pmw3901mb_shutdown");
            pmw3901mb_shutdown(gs);
        }
        else
        {
            LOG_D("pmw3901mb_power_up");
            pmw3901mb_power_up(gs);
            pmw3901mb_set_optimum_performance(gs);
        }
    }
    break;
    default:
        break;
    }
    return result;
}


static struct rt_sensor_ops pmw3901mb_ops =
{
    pmw3901mb_fect_data,
    pmw3901mb_control,
};

int init_optical_sensor(void)
{
    rt_int8_t result;
    rt_sensor_t sensor = RT_NULL;
    pmw3901mb_handle_t *handle=_pmw3901mb_creat();
    if(RT_NULL == handle)
    {
        LOG_E("fail to creat pmw3901 dev");
        return -2;

    }
    sensor = rt_calloc(1, sizeof(struct rt_sensor_device));
    if(RT_NULL == sensor)
    {
        LOG_E("fail to rt_calloc");
        return -1;
    }
    sensor->info.type = RT_SENSOR_CLASS_OP;
    sensor->info.vendor = RT_SENSOR_VENDOR_PIXART;
    sensor->info.model = "pmw3901mb";
    sensor->info.unit = RT_SENSOR_UNIT_PIXEL;
    sensor->info.intf_type = RT_SENSOR_INTF_SPI;
    sensor->info.range_max = 120;
    sensor->info.range_min = -120;
    sensor->info.period_min= 9;
    sensor->info.fifo_max = 0;

    sensor->ops = &pmw3901mb_ops;


    result = rt_hw_sensor_register(sensor, "pmw3901", RT_DEVICE_FLAG_RDWR, handle);

    return result;
}

int optical_sensor_init_task(void)
{
    uint8_t ret = 0;
    ret = init_optical_sensor();

    if(ret == RT_EOK)
    {
        FC_ERROR_RESET(FC_OPTICALFLOW_HARDFAULT);
        return RT_EOK;
    }
    else
    {
        FC_ERROR_RESET(FC_OPTICALFLOW_HARDFAULT);
    }
    return -RT_ERROR;
}

ZINO_APP_EXPORT(optical_sensor_init_task);